#include "cartographer/mapping/internal/2d/scan_matching/adaptive_scan_matcher_2d.h"

using namespace cartographer::mapping;
using namespace cartographer::mapping::scan_matching;

namespace cartographer
{
    AdaptiveWindow::AdaptiveWindow(const ScanMatchOptions &options)
    {
        inited_ = false;
        win_def_linear = options.linear_search_window();
        win_def_angular = options.angular_search_window();
        win_linear = win_def_linear;
        win_angular = win_def_angular;
        cost_weight_linear = options.translation_delta_cost_weight();
        cost_weight_angular = options.rotation_delta_cost_weight();
        LOG(INFO) << "[AdaptiveWindow] win_angular " << win_angular << " win_linear " << win_linear << " cost_weight_linear "
                  << cost_weight_linear << " cost_weight_angular " << cost_weight_angular;
    }

    std::vector<sensor::PointCloud> AdaptiveWindow::GenerateRotatedScans(const sensor::PointCloud &cloud)
    {
        std::vector<sensor::PointCloud> rotated_scans;
        rotated_scans.reserve(rotations_.size());
        /*double delta_theta = -angular_step_size * angular_stride;
        for (int index = 0; index < num_scans_; ++index, delta_theta += angular_stride)
        {
            rotated_scans.push_back(sensor::TransformPointCloud(
                point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(delta_theta, Eigen::Vector3f::UnitZ()))));
        }*/
        for (double rotate : rotations_)
        {
            rotated_scans.push_back(
                sensor::TransformPointCloud(cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(rotate, Eigen::Vector3f::UnitZ()))));
        }
        return rotated_scans;
    }

    void AdaptiveWindow::DiscretizeScans(std::vector<DiscreteScan2D> &discrete_scans, const MapLimits &map_limits,
                                         const std::vector<sensor::PointCloud> &scans, const double x, const double y)
    {
        Eigen::Translation2f translation(x, y);
        discrete_scans.reserve(scans.size());
        for (const sensor::PointCloud &scan : scans)
        {
            discrete_scans.emplace_back();
            discrete_scans.back().reserve(scan.size());
            for (const sensor::RangefinderPoint &point : scan)
            {
                const Eigen::Vector2f translated_point = Eigen::Affine2f(translation) * point.position.head<2>();
                discrete_scans.back().push_back(map_limits.GetCellIndex(translated_point));
            }
        }
    }

    void AdaptiveWindow::UpdateParameter(const float max_range, const double submap_resolution)
    {
        resolution_linear = 0.01;
        // resolution_linear = submap_resolution >= 0.02 ? 0.02 : submap_resolution;
        const double kSafetyMargin = 1. - 1e-3;
        // https://blog.csdn.net/weixin_44088154/article/details/133828291
        // 角度搜索步长
        // angular_stride = kSafetyMargin * std::acos(1. - common::Pow2(submap_resolution) / (2. * common::Pow2(max_range)));
        angular_stride = common::DegToRad(1.0);
        //  窗口内移动步数
        angular_step_size = std::ceil(win_angular / angular_stride);
        // 扩大角度移动步数
        num_scans_ = 2 * angular_step_size + 1;
        // XY方向的搜索步长
        linear_step_size = std::ceil(win_linear / resolution_linear);
        // 三个维度步长
        if (!inited_)
            offsetStride = Eigen::Vector3i(linear_step_size, linear_step_size, angular_step_size);

        // print message
        LOG(INFO) << "[AdaptiveParameters] : \n";
        LOG(INFO) << "num_scans " << num_scans_ << " map_resolution " << submap_resolution << " max_range " << max_range;
        LOG(INFO) << "angula: win " << common::RadToDeg(win_angular) << " stride " << common::RadToDeg(angular_stride);
        LOG(INFO) << "linear: win " << win_linear << " stride " << resolution_linear;
        PrintSpan("[InitSpan]");
    }

    Candidate2D AdaptiveWindow::GetCandidate(int id, int scan_index, int x_offset, int y_offset, double yaw)
    {
        Candidate2D cand;
        cand.id = id;
        cand.score = 0.0;
        cand.scan_index = scan_index;
        cand.x_index_offset = x_offset;
        cand.y_index_offset = y_offset;
        // 注意submap和局部系的坐标系是反向的，参考https://blog.csdn.net/qq_26675881/article/details/125941434
        //  这里的x和y是局部系的坐标值
        cand.x += -y_offset * resolution_linear;
        cand.y += -x_offset * resolution_linear;
        cand.orientation = yaw;
        // 偏航角度是按照负值往正值排列的
        // cand.orientation = (scan_index - angular_step_size) * angular_stride;
        return cand;
    }

    void AdaptiveWindow::Reset(const int num_rotation, const int num_cand)
    {
        num_candidates = num_cand;

        if (!rotations_.empty())
            std::vector<double>().swap(rotations_);
        rotations_.reserve(num_rotation);

        if (!weights_.empty())
            std::vector<double>().swap(weights_);
        if (num_candidates > 0)
            weights_.reserve(num_candidates);

        if (!candidates.empty())
            std::vector<Candidate2D>().swap(candidates);
        if (num_candidates > 0)
            candidates.reserve(num_candidates);

        // 偏移量误差的方差
        sigma_ = Eigen::Vector3d(0.001, 0.001, 0.001);
    }

    void AdaptiveWindow::ResetLinearBounds(const int num_rotate_scans)
    {
        if (!linear_bounds.empty())
            std::vector<LinearBounds>().swap(linear_bounds);
        for (int i = 0; i < num_rotate_scans; ++i)
            linear_bounds.emplace_back(-offsetStride(0), offsetStride(0), -offsetStride(1), offsetStride(1));
    }

    void AdaptiveWindow::SampleExhaustive()
    {
        // 设置平移边界
        ResetLinearBounds(num_scans_);

        // 预估候选集大小
        int evaluateSize = 0;
        for (int i = 0; i < num_scans_; ++i)
        {
            const int num_x = (linear_bounds[i].max_x - linear_bounds[i].min_x + 1);
            const int num_y = (linear_bounds[i].max_y - linear_bounds[i].min_y + 1);
            evaluateSize += num_x * num_y;
        }
        Reset(num_scans_, evaluateSize);

        // 偏航角由负到正
        double deltaYaw = -angular_step_size * angular_stride;
        for (int i = 0; i < num_scans_; ++i, deltaYaw += angular_stride)
            rotations_.push_back(deltaYaw);

        SampleCommon();

        // CHECK_EQ(candidates.size(), num_candidates);
        LOG(INFO) << "[SampleExhaustive] size rot " << rotations_.size() << " cand " << num_candidates << " " << candidates.size();
    }

    void AdaptiveWindow::PrintSpan(const char *name)
    {
        auto span_x = resolution_linear * offsetStride(0);
        auto span_y = resolution_linear * offsetStride(1);
        auto span_angular = angular_stride * offsetStride(2);
        LOG(INFO) << name << " off x " << offsetStride(0) << " y " << offsetStride(1) << " yaw " << offsetStride(2);
        LOG(INFO) << name << " span x " << span_x << " y " << span_y << " yaw " << common::RadToDeg(span_angular);
    }

    void AdaptiveWindow::ResetSampleOffsetStride1()
    {
        int keepSize = int(candidates.size());
        float halfSize = float(num_candidates) * 0.1;
        float expSampleSize = halfSize / float(keepSize);
        int sampleSizePer = std::pow(expSampleSize, 1.0 / 3.0);
        int sampleSizeSide = sampleSizePer * 0.5;

        // 设置新的偏移量
        offsetStride = Eigen::Vector3i(sampleSizeSide, sampleSizeSide, sampleSizeSide);
        LOG(INFO) << "[SampleNears] keepSize " << keepSize << " halfSize " << halfSize << " num_candidates " << num_candidates;
        LOG(INFO) << "[SampleNears] sampleSize: exp " << expSampleSize << " per " << sampleSizePer << " side " << sampleSizeSide;

        auto span_x = resolution_linear * offsetStride(0);
        auto span_y = resolution_linear * offsetStride(1);
        auto span_angular = angular_stride * offsetStride(2);
        LOG(INFO) << "[SampleNears] span x " << span_x << " y " << span_y << " yaw " << common::RadToDeg(span_angular);
    }

    bool AdaptiveWindow::ResetSampleOffsetStride()
    {
        offsetStride = Eigen::Vector3i(2, 2, std::ceil((win_angular * 0.5) / angular_stride));
        Eigen::Vector3d deltaAbs(std::fabs(offsetDelta_(0)), std::fabs(offsetDelta_(1)), common::RadToDeg(std::fabs(offsetDelta_(2))));
        if (deltaAbs(0) > 0.03 || deltaAbs(1) > 0.03 || deltaAbs(2) > 2.0)
            return false;
        return true;
    }

    void AdaptiveWindow::UpdateRotateByCandidates()
    {
        double firstAngle = candidates[0].orientation + -offsetStride(2) * angular_stride;
        Eigen::Vector2d limit(firstAngle, firstAngle);

        std::vector<double> orientations;
        for (size_t i = 0; i < candidates.size(); i++)
        {
            const auto &cand = candidates[i];
            for (int iw = -offsetStride(2); iw <= offsetStride(2); iw++)
            {
                double rotate = cand.orientation + iw * angular_stride;
                if (rotate > limit(1))
                    limit(1) = rotate;
                if (rotate < limit(0))
                    limit(0) = rotate;
                orientations.push_back(rotate);
            }
        }

        int group_size = std::ceil(std::fabs(limit(1) - limit(0)) / angular_stride);
        std::vector<int> histogram(group_size, 0);
        for (size_t i = 0; i < orientations.size(); i++)
        {
            int group_index = (orientations[i] - limit(0) + 0.0001) / angular_stride;
            if (group_index > group_size)
                continue;
            histogram[group_index]++;
        }
        if (!rotations_.empty())
            std::vector<double>().swap(rotations_);
        for (size_t i = 0; i < histogram.size(); i++)
        {
            if (histogram[i] < 1)
                continue;
            rotations_.push_back(double(i) * angular_stride + limit(0) - 0.0001);
        }
    }

    void AdaptiveWindow::UpdateOffsetByCandidates(std::vector<Eigen::Vector2d> &keeps)
    {
        Eigen::Vector2d min;
        Eigen::Vector2d max;
        int cnt = 0;
        std::vector<Eigen::Vector2d> offsets;
        for (size_t i = 0; i < candidates.size(); i++)
        {
            const auto &cand = candidates[i];
            for (int ix = -offsetStride(0); ix <= offsetStride(0); ix++)
            {
                for (int iy = -offsetStride(1); iy <= offsetStride(1); iy++)
                {
                    double x = cand.x - iy * resolution_linear;
                    double y = cand.y - ix * resolution_linear;
                    offsets.push_back(Eigen::Vector2d(x, y));
                    if (cnt == 0)
                    {
                        min = Eigen::Vector2d(x, y);
                        max = Eigen::Vector2d(x, y);
                    }
                    else
                    {
                        if (x < min(0))
                            min(0) = x;
                        if (y < min(1))
                            min(1) = y;

                        if (x > max(0))
                            max(0) = x;
                        if (y > max(1))
                            max(1) = y;
                    }
                    cnt++;
                }
            }
        }

        Eigen::Vector2d len = max - min;
        Eigen::Vector2i cells;
        cells(0) = len(0) / resolution_linear;
        cells(1) = len(1) / resolution_linear;

        std::vector<std::vector<bool>> flags(cells(0));
        for (auto &item : flags)
        {
            item.resize(cells(1), false);
        }
        for (size_t i = 0; i < offsets.size(); i++)
        {
            offsets[i] = offsets[i] - min;
            offsets[i](0) += 0.0001;
            offsets[i](1) += 0.0001;
        }

        for (size_t i = 0; i < offsets.size(); i++)
        {
            const auto &off = offsets[i];
            int idx_x = off(0) / resolution_linear;
            int idx_y = off(1) / resolution_linear;
            if (idx_x >= cells(0) || idx_y >= cells(1))
                continue;
            flags[idx_x][idx_y] = true;
        }

        for (int x = 0; x < cells(0); x++)
        {
            for (int y = 0; y < cells(1); y++)
            {
                if (flags[x][y])
                {
                    double cenx = (x + 1) * resolution_linear * 0.5;
                    double ceny = (y + 1) * resolution_linear * 0.5;
                    cenx += min(0);
                    ceny += min(1);
                    keeps.push_back(Eigen::Vector2d(cenx, ceny));
                }
            }
        }
    }

    void AdaptiveWindow::SampleNears()
    {
        PrintSpan("[NearSpan]");

        UpdateRotateByCandidates();
        std::vector<Eigen::Vector2d> keepOffsets;
        UpdateOffsetByCandidates(keepOffsets);

        if (!candidates.empty())
            std::vector<Candidate2D>().swap(candidates);

        idCnt_ = 0;
        for (size_t i = 0; i < rotations_.size(); ++i)
        {
            for (const auto &offset : keepOffsets)
            {
                candidates.emplace_back(idCnt_, i, offset(0), offset(1), rotations_[i]);
                idCnt_++;
            }
        }

        weights_.resize(idCnt_, 1.0 / double(idCnt_));

        LOG(INFO) << "[SampleNears] candSize " << candidates.size() << " idCnt " << idCnt_;
        LOG(INFO) << "[SampleNears] size rot " << rotations_.size() << " offset " << keepOffsets.size();
    }

    void AdaptiveWindow::SampleCommon()
    {
        CHECK_EQ(rotations_.size(), linear_bounds.size());
        idCnt_ = 0;
        for (size_t i = 0; i < rotations_.size(); ++i)
        {
            for (int x_offset = linear_bounds[i].min_x; x_offset <= linear_bounds[i].max_x; ++x_offset)
            {
                for (int y_offset = linear_bounds[i].min_y; y_offset <= linear_bounds[i].max_y; ++y_offset)
                {
                    candidates.emplace_back(idCnt_, i, x_offset, y_offset, rotations_[i], resolution_linear);
                    idCnt_++;
                }
            }
        }
        weights_.resize(idCnt_, 1.0 / double(idCnt_));
    }

    void AdaptiveWindow::SampleNearsOnlyYaw()
    {
        PrintSpan("[NearSpan]");

        UpdateRotateByCandidates();

        // 设置平移边界
        ResetLinearBounds(rotations_.size());

        if (!candidates.empty())
            std::vector<Candidate2D>().swap(candidates);

        SampleCommon();

        LOG(INFO) << "[SampleNears] size rot " << rotations_.size() << " cand " << idCnt_ << " " << candidates.size();
    }

    void AdaptiveWindow::UpdateWeightByScore()
    {
        best_idx_ = 0;
        score_max_ = 0;
        float weight_sum = 0;
        for (size_t i = 0; i < candidates.size(); i++)
        {
            float score = candidates[i].score;
            weight_sum += score;
            weights_[i] = score;
            if (i == 0 || score > score_max_)
            {
                best_idx_ = i;
                score_max_ = score;
            }
        }
        NormalizeWeight(weights_, weight_sum);
        const auto &best = candidates[best_idx_];
        offsetBest = Eigen::Vector3d(best.x, best.y, best.orientation);
        PrintCandidate("[bestcand]", best);
    }

    void AdaptiveWindow::UpdateWeightByExp()
    {
        double weight_sum = 0;
        Eigen::Vector3d delta;
        delta.setZero();
        std::vector<double> exp_weights;
        int sizeCand = int(candidates.size());

        int maxIdx = 0;
        double weightMax = 0;
        exp_weights.resize(sizeCand, 0.0);
        for (int i = 0; i < sizeCand; i++)
        {
            auto &cand = candidates[i];
            delta(0) = cand.x - offsetExpect(0);
            delta(1) = cand.y - offsetExpect(1);
            delta(2) = cand.orientation - offsetExpect(2);
            double weight = 1.0 * std::exp(-(delta(2) * delta(2)) / 2 * sigma_(2) * sigma_(2));
            if (weight >= 1.0)
            {
                LOG(INFO) << " weight 1.0 " << delta(2) << " " << sigma_(2);
            }
            // double weight = ComputeWeightRatio(delta(0), delta(1), delta(2));
            // double weight = GaussProbability(delta, sigma_);
            exp_weights[i] = weight;
            weight_sum += weight;
            if (i == 0 || weight > weightMax)
            {
                maxIdx = i;
                weightMax = weight;
            }
        }
        LOG(INFO) << "============";
        NormalizeWeight(exp_weights, weight_sum);
        PrintMaxWeightInfo(maxIdx, exp_weights[maxIdx]);

        maxIdx = 0;
        weightMax = 0;
        weight_sum = 0;
        double alpha = 0.0;
        for (int i = 0; i < sizeCand; i++)
        {
            candidates[i].weight_score = weights_[i];
            candidates[i].weight_exp = exp_weights[i];
            double weight = weights_[i] * alpha + (1 - alpha) * exp_weights[i];
            weight_sum += weight;
            weights_[i] = weight;
            if (i == 0 || weight > weightMax)
            {
                maxIdx = i;
                weightMax = weight;
            }
        }
        NormalizeWeight(weights_, weight_sum);
        PrintMaxWeightInfo(maxIdx, weights_[maxIdx]);
    }

    void AdaptiveWindow::PrintMaxWeightInfo(int maxIdx, double weightMax)
    {
        auto &candBest = candidates[maxIdx];
        double deltax = candBest.x - offsetExpect(0);
        double deltay = candBest.y - offsetExpect(1);
        double deltayaw = candBest.orientation - offsetExpect(2);
        LOG(INFO) << "[MaxWeightInfo] idx " << maxIdx << " " << deltax << " " << deltay << " " << deltayaw << " weight " << weightMax;
    }

    void AdaptiveWindow::NormalizeWeight(std::vector<double> &weights, const double weight_sum)
    {
        if (weights_.size() != candidates.size())
            LOG(INFO) << "wrong size : weights " << weights_.size() << " candidates " << candidates.size();
        for (size_t i = 0; i < candidates.size(); i++)
            weights_[i] = weights_[i] / weight_sum;
    }

    int AdaptiveWindow::Resample()
    {
        return -1;
        double weight_max = 0;
        double weight_min = 0;
        for (size_t i = 0; i < weights_.size(); ++i)
        {
            double weight = weights_[i];
            if (i == 0 || weight_max < weight)
                weight_max = weight;
            if (i == 0 || weight_min > weight)
                weight_min = weight;
        }
        // double weight_limit = (weight_max - weight_min) * 0.9 + weight_min;
        double weight_limit = (weight_max + weight_min) * 0.5;

        int groupSize = 10;
        int weight_size = 0;
        double weight_sum = 0.0;
        float groupWidth = (weight_max - weight_limit) / float(groupSize);
        std::vector<HistogramGroup> groups;
        groups.resize(groupSize + 1);

        LOG(INFO) << "wrong weight " << weight_max << " " << weight_limit << " groupWidth " << groupWidth;
        for (size_t i = 0; i < weights_.size(); ++i)
        {
            double weight = weights_[i];
            if (weight < weight_limit)
                continue;
            weight_sum += weight;
            weight_size++;
            // 3 4 5 6.6 1.2
            // 0 1 2 3.6
            int gpIndex = (weight - weight_limit) / groupWidth;
            if (gpIndex > groupSize || gpIndex < 0)
            {
                LOG(INFO) << "wrong group " << i << " index " << gpIndex << " weight " << weight;
                continue;
            }
            auto &gpItem = groups[gpIndex];
            if (gpItem.id < 0)
            {
                gpItem.id = gpIndex;
                gpItem.count = i;
                gpItem.value = weight;
            }
            else
            {
                if (gpItem.value < weight)
                {
                    gpItem.count = i;
                    gpItem.value = weight;
                }
            }
            // const auto &candKp = candidates[i];
            // keepCands.push_back(candidates[i]);
        }

        double weight_average = weight_sum / double(weight_size);
        if (weight_slow_ < 0.0)
            weight_slow_ = weight_average;
        else
            weight_slow_ = alpha_slow_ * weight_average + (1 - alpha_slow_) * weight_slow_;

        if (weight_fast_ < 0.0)
            weight_fast_ = weight_average;
        else
            weight_fast_ = alpha_fast_ * weight_average + (1 - alpha_fast_) * weight_fast_;

        double weight_diff = 1.0 - weight_fast_ / weight_slow_;
        if (weight_diff > 0.5)
            return -1;
        std::vector<Candidate2D> keepCands;
        for (const auto &gp : groups)
        {
            if (gp.id < 0)
                continue;
            keepCands.push_back(candidates[gp.count]);
        }
        if (keepCands.size() < 1)
            return -2;
        LOG(INFO) << "[Resample] keep " << keepCands.size() << " limit " << weight_limit << " min " << weight_min << " max " << weight_max;
        // reset data
        candidates.swap(keepCands);
        weights_.clear();
        rotations_.clear();
        return 1;
    }

    void AdaptiveWindow::ScoreCandidates(const Grid2D &grid, const std::vector<DiscreteScan2D> &discrete_scans)
    {
        for (auto &cand : candidates)
        {
            // PerfSet::Reset("ComputeCandidateScore");
            switch (grid.GetGridType())
            {
            case GridType::PROBABILITY_GRID:
                cand.score = ComputeCandidateScore(static_cast<const ProbabilityGrid &>(grid), discrete_scans[cand.scan_index],
                                                   cand.x_index_offset, cand.y_index_offset);
                break;
            case GridType::TSDF:
                cand.score = ComputeCandidateScore(static_cast<const TSDF2D &>(grid), discrete_scans[cand.scan_index], cand.x_index_offset,
                                                   cand.y_index_offset);
                break;
            }
            float range = std::hypot(cand.x, cand.y);
            cand.score *= std::exp(-common::Pow2(range * cost_weight_linear + std::abs(cand.orientation) * cost_weight_angular));
            // PerfSet::Update("ComputeCandidateScore", true);
        }
        auto pointSize = discrete_scans[0].size();
        auto visitTimes = candidates.size() * pointSize;
        LOG(INFO) << "visittimes2 " << visitTimes << " pointSize " << pointSize;
    }

    double AdaptiveWindow::ComputeWeightRatio(const double x, const double y, const double yaw)
    {
        // 距离和角度偏差越大，权重系数越小
        return std::exp(-common::Pow2(std::hypot(x, y) * cost_weight_linear + std::abs(yaw) * cost_weight_angular));
    }

    void AdaptiveWindow::ScoreCandidates(const Grid2D &grid, const std::vector<sensor::PointCloud> &rotated_clouds, const double x,
                                         const double y)
    {
        int scoreWrongSize = 0;
        LOG(INFO) << " ScoreCandidatesInput " << x << " " << y << " candSize " << candidates.size();
        CandidateScoreComputor2D computor(grid);
        for (auto &cand : candidates)
        {
            cand.score = computor.Compute(rotated_clouds[cand.scan_index], cand.scan_index, x + cand.x, y + cand.y);
            if (cand.score <= 0.0)
            {
                scoreWrongSize++;
                continue;
            }
            cand.weight_ratio = ComputeWeightRatio(cand.x, cand.y, cand.orientation);
            cand.score *= cand.weight_ratio;
        }
        LOG(INFO) << "scoreWrongSize " << scoreWrongSize;
        auto pointSize = rotated_clouds[0].size();
        auto visitTimes = candidates.size() * pointSize;
        LOG(INFO) << "visittimes " << visitTimes << " pointSize " << pointSize;
    }

    void AdaptiveWindow::Failed(const transform::Rigid2d &pose)
    {
        inited_ = false;
        Reset(0, 0);
        offsetDeltaHis_.clear();
    }

    void AdaptiveWindow::UpdateSigma(const Eigen::Vector3d &delta)
    {
        while (offsetDeltaHis_.size() > 9)
            offsetDeltaHis_.pop_front();
        offsetDeltaHis_.push_back(delta);

        auto hisSize = offsetDeltaHis_.size();
        if (hisSize > 9)
        {
            Eigen::Vector3d aver(0, 0, 0);
            for (const auto &rec : offsetDeltaHis_)
                aver += rec;
            aver /= (double)hisSize;

            Eigen::Vector3d cov(0, 0, 0);
            for (const auto &rec : offsetDeltaHis_)
            {
                const auto delta = rec - aver;
                cov(0) += std::pow(delta(0), 2);
                cov(1) += std::pow(delta(1), 2);
                cov(2) += std::pow(delta(2), 2);
            }
            cov /= (double)hisSize;
            sigma_ = cov;
        }
    }

    int AdaptiveWindow::Feedback(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt)
    {
        offsetExpect = ComputeOffset(pose_init, pose_opt);

        LOG(INFO) << "[Feedback] org size " << candidates.size();
        LOG(INFO) << "[Feedback] offset_exp " << offsetExpect(0) << " " << offsetExpect(1) << " " << common::RadToDeg(offsetExpect(2));
        LOG(INFO) << "[Feedback] offset_est " << offsetBest(0) << " " << offsetBest(1) << " " << common::RadToDeg(offsetBest(2));

        offsetDelta_ = offsetExpect - offsetBest;
        LOG(INFO) << "[Feedback] deltax " << offsetDelta_(0);
        LOG(INFO) << "[Feedback] deltay " << offsetDelta_(1);
        LOG(INFO) << "[Feedback] deltayaw " << common::RadToDeg(offsetDelta_(2));

        if (candidates.empty())
        {
            LOG(INFO) << "[Feedback] candidates.empty ";
            inited_ = false;
            return -1;
        }

        // UpdateSigma(offsetDelta_);

        UpdateWeightByExp();

        if (!ResetSampleOffsetStride())
        {
            inited_ = false;
            return -2;
        }
        if (!Resample())
        {
            inited_ = false;
            LOG(INFO) << "[Feedback] resample failed";
            return -3;
        }
        inited_ = true;

        Candidate2D cand;
        cand.id = -1;
        cand.score = 0.0;
        cand.scan_index = -1;
        cand.x_index_offset = 0;
        cand.y_index_offset = 0;
        cand.x = offsetExpect(0);
        cand.y = offsetExpect(1);
        cand.orientation = offsetExpect(2);
        // candidates.push_back(cand);
        LOG(INFO) << "[Feedback] resample size " << candidates.size();
        return 1;
    }

    bool AdaptiveWindow::Check()
    {
        if (!inited_ && rotations_.size() != size_t(num_scans_))
        {
            LOG(INFO) << "[Adaptive] wrong rotations size " << rotations_.size() << " num_scans " << num_scans_;
            return false;
        }
        if (inited_ && rotations_.size() < 1)
        {
            LOG(INFO) << "[Adaptive] wrong rotations size " << rotations_.size() << " num_scans" << num_scans_;
            return false;
        }
        if (candidates.size() < 1)
        {
            LOG(INFO) << "[Adaptive] candidates empty ";
            return false;
        }
        return true;
    }

    AdaptiveScanMatcher2D::AdaptiveScanMatcher2D(const ScanMatchOptions &options)
        : options_(options)
    {
        filter_ = std::make_shared<AdaptiveWindow>(options);
    }

    double AdaptiveScanMatcher2D::Match(const transform::Rigid2d &poseInit, const sensor::PointCloud &point_cloud, const Grid2D &grid,
                                        transform::Rigid2d *pose_estimate)
    {
        CHECK(pose_estimate != nullptr);
        // transform
        float yaw = poseInit.rotation().cast<float>().angle();
        double trans_x = poseInit.translation().x();
        double trans_y = poseInit.translation().y();
        auto transform = transform::Rigid3f::Rotation(Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
        const auto rotated_cloud = sensor::TransformPointCloud(point_cloud, transform);
        LOG(INFO) << "[Match] initpose x " << trans_x << " y " << trans_y << " yaw " << common::RadToDeg(yaw);

        // 更新参数
        PerfSet::Reset("UpdateParameter");
        float gridResolution = grid.limits().resolution();
        float rangeMax = ComputeRangeMax(rotated_cloud, gridResolution);
        filter_->UpdateParameter(rangeMax, gridResolution);
        PerfSet::Update("UpdateParameter", true);

        // 在x和y的维采样若干候选位姿，偏移量是指栅格偏移数
        PerfSet::Reset("Sample");
        if (filter_->inited_)
            filter_->SampleNearsOnlyYaw();
        else
        {
            filter_->SampleExhaustive();
        }
        PerfSet::Update("Sample", true);
        if (!filter_->Check())
        {
            filter_->inited_ = false;
            return 0.0;
        }
        PerfSet::Reset("RotatedScan");
        // 以初始值为基础，生成一些列的旋转后的点云
        const auto rotated_scans = filter_->GenerateRotatedScans(rotated_cloud);
        PerfSet::Update("RotatedScan", true);
        LOG(INFO) << "[AdaptiveScan] size rots " << filter_->rotations_.size() << " cand " << filter_->candidates.size() << " scan "
                  << rotated_scans.size();
        // 计算每个候选位姿的概率得分
        if (filter_->inited_)
        {
            PerfSet::Reset("ScoreCandidate");
            filter_->ScoreCandidates(grid, rotated_scans, trans_x, trans_y);
            PerfSet::Update("ScoreCandidate", true);
        }
        else
        {
            PerfSet::Reset("DiscretizeScans");
            // 根据旋转点云坐标，生成点对应的栅格坐标
            std::vector<DiscreteScan2D> discrete_scans;
            filter_->DiscretizeScans(discrete_scans, grid.limits(), rotated_scans, trans_x, trans_y);
            PerfSet::Update("DiscretizeScans", true);
            // score
            PerfSet::Reset("ScoreCandidate2");
            filter_->ScoreCandidates(grid, discrete_scans);
            PerfSet::Update("ScoreCandidate2", true);
            LOG(INFO) << "[AdaptiveScan] discrete_scans " << discrete_scans.size();
        }
        // update weight
        PerfSet::Reset("UpdateWeight");
        filter_->UpdateWeightByScore();
        PerfSet::Update("UpdateWeight", true);

        // 选出最佳得分的候选位姿作为返回结果
        // const Candidate2D &best = *std::max_element(candidates.begin(), candidates.end());
        const Candidate2D &best = filter_->candidates[filter_->best_idx_];
        const auto rotation = poseInit.rotation() * Eigen::Rotation2Dd(best.orientation);
        *pose_estimate = transform::Rigid2d({trans_x + best.x, trans_y + best.y}, rotation);
        return best.score;
    }
} // namespace cartographer
